/*
 * Copyright (C) 2016 Nishant Srivastava
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package com.github.nisrulz.sensey;


import ohos.sensor.bean.CategoryEnvironment;
import ohos.sensor.bean.CategoryMotion;
import ohos.sensor.data.CategoryOrientationData;
import ohos.sensor.data.SensorData;

public class OrientationDetector extends SensorDetector {

    public interface OrientationListener {

        void onBottomSideUp();

        void onLeftSideUp();

        void onRightSideUp();

        void onTopSideUp();
    }

    private static final int ORIENTATION_PORTRAIT = 6; // 6
    private static final int ORIENTATION_LANDSCAPE_REVERSE = 3;// 3
    private static final int ORIENTATION_LANDSCAPE = 1; // 1
    private static final int ORIENTATION_PORTRAIT_REVERSE = 8; // 8

    private float averagePitch = 0;

    private float averageRoll = 0;

    private int eventOccurred = 0;

    private float[] mGeomagnetic;

    private float[] mGravity;

    private int orientation = ORIENTATION_PORTRAIT;

    private final OrientationListener orientationListener;

    private final float[] pitches;

    private final float[] rolls;

    private final int smoothness;

    public OrientationDetector(OrientationListener orientationListener) {
        this(1, orientationListener);
    }

    public OrientationDetector(int smoothness, OrientationListener orientationListener) {
        super(SensorType.SENSOR_TYPE_ACCELEROMETER, SensorType.SENSOR_TYPE_MAGNETIC_FIELD);
        this.smoothness = smoothness;
        this.orientationListener = orientationListener;

        pitches = new float[smoothness];
        rolls = new float[smoothness];
    }

    @Override
    protected void onSensorEvent(SensorData event) {
        if (event.sensor instanceof CategoryMotion) {
            mGravity = event.values;
        }
        if (event.sensor instanceof CategoryEnvironment) {
            mGeomagnetic = event.values;
        }
        if (mGravity != null && mGeomagnetic != null) {
            float[] orientationData = new float[3];
//            float[] rotationMatrix = new float[16];
//            CategoryOrientationData.getDeviceRotationMatrix(rotationMatrix, mGravity);
            float[] R = new float[9];
            float[] I = new float[9];
            boolean success = getRotationMatrix(R,I,mGravity,mGeomagnetic);
            if (!success) return;
            orientationData = CategoryOrientationData.getDeviceOrientation(R, orientationData);
            averagePitch = addValue(orientationData[1], pitches);
            averageRoll = addValue(orientationData[2], rolls);
            orientation = calculateOrientation();
            switch (orientation) {
                case ORIENTATION_PORTRAIT:
                    if (eventOccurred != 1) {
                        eventOccurred = 1;
                        orientationListener.onTopSideUp();
                    }
                    break;
                case ORIENTATION_LANDSCAPE:
                    if (eventOccurred != 2) {
                        eventOccurred = 2;
                        orientationListener.onRightSideUp();
                    }
                    break;
                case ORIENTATION_PORTRAIT_REVERSE:
                    if (eventOccurred != 3) {
                        eventOccurred = 3;
                        orientationListener.onBottomSideUp();
                    }
                    break;
                case ORIENTATION_LANDSCAPE_REVERSE:
                    if (eventOccurred != 4) {
                        eventOccurred = 4;
                        orientationListener.onLeftSideUp();
                    }
                    break;
                default:
                    // do nothing
                    break;
            }
//            }
        }
    }

    private float addValue(float value, float[] values) {
        float temp_value = (float) Math.round(Math.toDegrees(value));
        float average = 0;
        for (int i = 1; i < smoothness; i++) {
            values[i - 1] = values[i];
            average += values[i];
        }
        values[smoothness - 1] = temp_value;
        average = (average + temp_value) / smoothness;
        return average;
    }

    private int calculateOrientation() {
        // finding local orientation dip
        if ((orientation == ORIENTATION_PORTRAIT || orientation == ORIENTATION_PORTRAIT_REVERSE) && (
                averageRoll > -30
                        && averageRoll < 30)) {
            if (averagePitch > 0) {
                return ORIENTATION_PORTRAIT_REVERSE;
            } else {
                return ORIENTATION_PORTRAIT;
            }
        } else {
            // divides between all orientations
            if (Math.abs(averagePitch) >= 30) {
                if (averagePitch > 0) {
                    return ORIENTATION_PORTRAIT_REVERSE;
                } else {
                    return ORIENTATION_PORTRAIT;
                }
            } else {
                if (averageRoll > 0) {
                    return ORIENTATION_LANDSCAPE_REVERSE;
                } else {
                    return ORIENTATION_LANDSCAPE;
                }
            }
        }
    }

    public  boolean getRotationMatrix(float[] R, float[] I,
                                            float[] gravity, float[] geomagnetic) {
        float Ax = gravity[0];
        float Ay = gravity[1];
        float Az = gravity[2];

        final float normsqA = (Ax * Ax + Ay * Ay + Az * Az);
        final float g = 9.81f;
        final float freeFallGravitySquared = 0.01f * g * g;
        if (normsqA < freeFallGravitySquared) {
            // gravity less than 10% of normal value
            return false;
        }

        final float Ex = geomagnetic[0];
        final float Ey = geomagnetic[1];
        final float Ez = geomagnetic[2];
        float Hx = Ey * Az - Ez * Ay;
        float Hy = Ez * Ax - Ex * Az;
        float Hz = Ex * Ay - Ey * Ax;
        final float normH = (float) Math.sqrt(Hx * Hx + Hy * Hy + Hz * Hz);

        if (normH < 0.1f) {
            // device is close to free fall (or in space?), or close to
            // magnetic north pole. Typical values are  > 100.
            return false;
        }
        final float invH = 1.0f / normH;
        Hx *= invH;
        Hy *= invH;
        Hz *= invH;
        final float invA = 1.0f / (float) Math.sqrt(Ax * Ax + Ay * Ay + Az * Az);
        Ax *= invA;
        Ay *= invA;
        Az *= invA;
        final float Mx = Ay * Hz - Az * Hy;
        final float My = Az * Hx - Ax * Hz;
        final float Mz = Ax * Hy - Ay * Hx;
        if (R != null) {
            if (R.length == 9) {
                R[0] = Hx;     R[1] = Hy;     R[2] = Hz;
                R[3] = Mx;     R[4] = My;     R[5] = Mz;
                R[6] = Ax;     R[7] = Ay;     R[8] = Az;
            } else if (R.length == 16) {
                R[0]  = Hx;    R[1]  = Hy;    R[2]  = Hz;   R[3]  = 0;
                R[4]  = Mx;    R[5]  = My;    R[6]  = Mz;   R[7]  = 0;
                R[8]  = Ax;    R[9]  = Ay;    R[10] = Az;   R[11] = 0;
                R[12] = 0;     R[13] = 0;     R[14] = 0;    R[15] = 1;
            }
        }
        if (I != null) {
            // compute the inclination matrix by projecting the geomagnetic
            // vector onto the Z (gravity) and X (horizontal component
            // of geomagnetic vector) axes.
            final float invE = 1.0f / (float) Math.sqrt(Ex * Ex + Ey * Ey + Ez * Ez);
            final float c = (Ex * Mx + Ey * My + Ez * Mz) * invE;
            final float s = (Ex * Ax + Ey * Ay + Ez * Az) * invE;
            if (I.length == 9) {
                I[0] = 1;     I[1] = 0;     I[2] = 0;
                I[3] = 0;     I[4] = c;     I[5] = s;
                I[6] = 0;     I[7] = -s;     I[8] = c;
            } else if (I.length == 16) {
                I[0] = 1;     I[1] = 0;     I[2] = 0;
                I[4] = 0;     I[5] = c;     I[6] = s;
                I[8] = 0;     I[9] = -s;     I[10] = c;
                I[3] = I[7] = I[11] = I[12] = I[13] = I[14] = 0;
                I[15] = 1;
            }
        }
        return true;
    }

}

